function [IK1,IK21,IK22,IK3] = ik_kuka(T1,T21,T22,T3,Tool,Fa)
%IK_KUKA 计算机器人关节角度
%   Tool=260;Fa=5
%%
%左封头
Tool1=zeros(length(T1),1);
for i=1:length(T1)
Tool1(i)=Tool+(i-1)*(Fa/length(T1));
end
i=1;
x=30;
[ik1(i,:),Error]=my_ik_theta7(T1(:,:,i),0,Tool1(i));
while Error>9.99999e-10
[ik1(i,:),Error]=my_ik_theta7(T1(:,:,i),0,Tool1(i));
end
qlim=[ik1(i,1)-x,ik1(i,1)+x;ik1(i,2)-x,ik1(i,2)+x;ik1(i,3)-x,ik1(i,3)+x;...
    ik1(i,4)-x,ik1(i,4)+x;ik1(i,5)-x,ik1(i,5)+x;ik1(i,6)-x,ik1(i,6)+x];
for i=2:length(T1)
[ik1(i,:),Error]=my_ik_theta7_g(T1(:,:,i),0,qlim,Tool1(i));
while Error>9.99999e-10
[ik1(i,:),Error]=my_ik_theta7_g(T1(:,:,i),0,qlim,Tool1(i));
end
qlim=[ik1(i,1)-x,ik1(i,1)+x;ik1(i,2)-x,ik1(i,2)+x;ik1(i,3)-x,ik1(i,3)+x;...
    ik1(i,4)-x,ik1(i,4)+x;ik1(i,5)-x,ik1(i,5)+x;ik1(i,6)-x,ik1(i,6)+x];
end
IK1=ik1;
%%
%筒身1
Tool21=Tool;
i=1;
x=30;
[ik21(i,:),Error]=my_ik_theta7(T21(:,:,i),0,Tool21);
while Error>9.99999e-10
[ik21(i,:),Error]=my_ik_theta7(T21(:,:,i),0,Tool21);
end
qlim=[ik21(i,1)-x,ik21(i,1)+x;ik21(i,2)-x,ik21(i,2)+x;ik21(i,3)-x,ik21(i,3)+x;...
    ik21(i,4)-x,ik21(i,4)+x;ik21(i,5)-x,ik21(i,5)+x;ik21(i,6)-x,ik21(i,6)+x];
for i=2:length(T21)
[ik21(i,:),Error]=my_ik_theta7_g(T21(:,:,i),0,qlim,Tool21);
while Error>9.99999e-10
[ik21(i,:),Error]=my_ik_theta7_g(T21(:,:,i),0,qlim,Tool21);
end
qlim=[ik21(i,1)-x,ik21(i,1)+x;ik21(i,2)-x,ik21(i,2)+x;ik21(i,3)-x,ik21(i,3)+x;...
    ik21(i,4)-x,ik21(i,4)+x;ik21(i,5)-x,ik21(i,5)+x;ik21(i,6)-x,ik21(i,6)+x];
end
IK21=ik21;
%%
%筒身2
Tool22=Tool;
i=1;
x=30;
[ik22(i,:),Error]=my_ik_theta7(T22(:,:,i),0,Tool22);
while Error>9.99999e-10
[ik22(i,:),Error]=my_ik_theta7(T22(:,:,i),0,Tool22);
end
qlim=[ik22(i,1)-x,ik22(i,1)+x;ik22(i,2)-x,ik22(i,2)+x;ik22(i,3)-x,ik22(i,3)+x;...
    ik22(i,4)-x,ik22(i,4)+x;ik22(i,5)-x,ik22(i,5)+x;ik22(i,6)-x,ik22(i,6)+x];
for i=2:length(T22)
[ik22(i,:),Error]=my_ik_theta7_g(T22(:,:,i),0,qlim,Tool22);
while Error>9.99999e-10
[ik22(i,:),Error]=my_ik_theta7_g(T22(:,:,i),0,qlim,Tool22);
end
qlim=[ik22(i,1)-x,ik22(i,1)+x;ik22(i,2)-x,ik22(i,2)+x;ik22(i,3)-x,ik22(i,3)+x;...
    ik22(i,4)-x,ik22(i,4)+x;ik22(i,5)-x,ik22(i,5)+x;ik22(i,6)-x,ik22(i,6)+x];
end
IK22=ik22;
%%
%右封头
Tool3=zeros(length(T3),1);
for i=1:length(T3)
Tool3(i)=Tool+(i-1)*(Fa/length(T3));
end
i=1;
x=30;
[ik3(i,:),Error]=my_ik_theta7(T3(:,:,i),0,Tool3(i));
while Error>9.99999e-10
[ik3(i,:),Error]=my_ik_theta7(T3(:,:,i),0,Tool3(i));
end
qlim=[ik3(i,1)-x,ik3(i,1)+x;ik3(i,2)-x,ik3(i,2)+x;ik3(i,3)-x,ik3(i,3)+x;...
    ik3(i,4)-x,ik3(i,4)+x;ik3(i,5)-x,ik3(i,5)+x;ik3(i,6)-x,ik3(i,6)+x];
for i=2:length(T3)
[ik3(i,:),Error]=my_ik_theta7_g(T3(:,:,i),0,qlim,Tool3(i));
while Error>9.99999e-10
[ik3(i,:),Error]=my_ik_theta7_g(T3(:,:,i),0,qlim,Tool3(i));
end
qlim=[ik3(i,1)-x,ik3(i,1)+x;ik3(i,2)-x,ik3(i,2)+x;ik3(i,3)-x,ik3(i,3)+x;...
    ik3(i,4)-x,ik3(i,4)+x;ik3(i,5)-x,ik3(i,5)+x;ik3(i,6)-x,ik3(i,6)+x];
end
IK3=ik3;
end